#include "h_cc2640r2f_iics.h"

#include <ti/drivers/PIN.h>
#include <ti/devices/cc26x0r2/driverlib/ioc.h>

#include "h_cc2640r2f_define.h"


typedef unsigned char                           IICS_PIN_NUM;

typedef struct
{
    IICS_PIN_NUM scl_pin;
    IICS_PIN_NUM sda_pin;
    PIN_Handle pin_outputhandle;
    PIN_State pin_outputstatus;
    unsigned short scltime;
}H_CC2640R2F_IICS_S;

// static PIN_Handle H_CC2640R2F_IICS_OutputHandle = NULL;
// static PIN_State H_CC2640R2F_IICS_OutputState;

#define H_CC2640R2F_IICS_SDA_H(x)                 PIN_setOutputValue(x->pin_outputhandle, x->sda_pin, 1)
#define H_CC2640R2F_IICS_SDA_L(x)                 PIN_setOutputValue(x->pin_outputhandle, x->sda_pin, 0)

#define H_CC2640R2F_IICS_SDA_Read(x)              PIN_getInputValue(x->sda_pin)

#define H_CC2640R2F_IICS_SCL_H(x)                  PIN_setOutputValue(x->pin_outputhandle, x->scl_pin, 1)
#define H_CC2640R2F_IICS_SCL_L(x)                  PIN_setOutputValue(x->pin_outputhandle, x->scl_pin, 0)

#define H_CC2640R2F_IICS_Delay(x)                 CPUdelay(x*12)

#define H_CC2640R2F_IICS_Debug                    H_CC2640R2F_UART_Debug

#define H_CC2640R2F_IICS_ReadOP                   0x01
#define H_CC2640R2F_IICS_WriteOP                  0x00

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

#define H_CC2640R2F_IICS0_SCL                     IOID_7
#define H_CC2640R2F_IICS0_SDA                     IOID_8
#define H_CC2640R2F_IICS0_SCL_Time                40//50//1000//1000//5

#define H_CC2640R2F_IICS1_SCL                     IOID_0
#define H_CC2640R2F_IICS1_SDA                     IOID_1
#define H_CC2640R2F_IICS1_SCL_Time                40//50//1000//1000//5


const PIN_Config H_CC2640R2F_IICS0_OutputCFG[] = 
{
    /* The Green LED on test board */
    H_CC2640R2F_IICS0_SCL | PIN_GPIO_OUTPUT_EN | PIN_GPIO_HIGH | PIN_PUSHPULL | PIN_DRVSTR_MAX,
    /* The Read LED on test board */
    H_CC2640R2F_IICS0_SDA | PIN_GPIO_OUTPUT_EN | PIN_GPIO_HIGH | PIN_PUSHPULL | PIN_DRVSTR_MAX,
    
    PIN_TERMINATE
};

const PIN_Config H_CC2640R2F_IICS1_OutputCFG[] = 
{
    /* The Green LED on test board */
    H_CC2640R2F_IICS1_SCL | PIN_GPIO_OUTPUT_EN | PIN_GPIO_HIGH | PIN_PUSHPULL | PIN_DRVSTR_MAX,
    /* The Read LED on test board */
    H_CC2640R2F_IICS1_SDA | PIN_GPIO_OUTPUT_EN | PIN_GPIO_HIGH | PIN_PUSHPULL | PIN_DRVSTR_MAX,
    
    PIN_TERMINATE
};

static H_CC2640R2F_IICS_S h_cc2640r2f_iics0_s;

static H_CC2640R2F_IICS_S h_cc2640r2f_iics1_s;

signed char H_CC2640R2F_IICS0_Init(void)
{
    h_cc2640r2f_iics0_s.scltime = H_CC2640R2F_IICS0_SCL_Time;
    h_cc2640r2f_iics0_s.scl_pin = H_CC2640R2F_IICS0_SCL;
    h_cc2640r2f_iics0_s.sda_pin = H_CC2640R2F_IICS0_SDA;

    if(NULL != h_cc2640r2f_iics0_s.pin_outputhandle)
    {
        return (-1);
    }
    h_cc2640r2f_iics0_s.pin_outputhandle = PIN_open(&h_cc2640r2f_iics0_s.pin_outputstatus, \
                                                H_CC2640R2F_IICS0_OutputCFG);
    
    
    return 0;
}

signed char H_CC2640R2F_IICS1_Init(void)
{
    h_cc2640r2f_iics1_s.scltime = H_CC2640R2F_IICS1_SCL_Time;
    h_cc2640r2f_iics1_s.scl_pin = H_CC2640R2F_IICS1_SCL;
    h_cc2640r2f_iics1_s.sda_pin = H_CC2640R2F_IICS1_SDA;

    if(NULL != h_cc2640r2f_iics1_s.pin_outputhandle)
    {
        return (-1);
    }
    h_cc2640r2f_iics1_s.pin_outputhandle = PIN_open(&h_cc2640r2f_iics1_s.pin_outputstatus, \
                                                H_CC2640R2F_IICS1_OutputCFG);
    
    
    return 0;
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

static void h_cc2640r2f_iics_sda_in(H_CC2640R2F_IICS_S *iics_s)
{
    PIN_setConfig(iics_s->pin_outputhandle, \
                    PIN_BM_OUTPUT_MODE | PIN_BM_INPUT_MODE, \
                        (iics_s->sda_pin) | PIN_GPIO_OUTPUT_DIS | PIN_INPUT_EN | PIN_PULLUP);
}

static void h_cc2640r2f_iics_sda_out(H_CC2640R2F_IICS_S *iics_s)
{
    PIN_setConfig(iics_s->pin_outputhandle, \
                    PIN_BM_OUTPUT_MODE | PIN_BM_INPUT_MODE, \
                        (iics_s->sda_pin) | PIN_GPIO_OUTPUT_EN | PIN_GPIO_LOW | PIN_PUSHPULL | \
                            PIN_DRVSTR_MAX);
}

static void h_cc2640r2f_iics_start(H_CC2640R2F_IICS_S *iics_s)
{
    H_CC2640R2F_IICS_SDA_L(iics_s);
    H_CC2640R2F_IICS_Delay(iics_s->scltime);
    H_CC2640R2F_IICS_SCL_L(iics_s);
}

static void h_cc2640r2f_iics_stop(H_CC2640R2F_IICS_S *iics_s)
{
    H_CC2640R2F_IICS_SCL_H(iics_s);
    H_CC2640R2F_IICS_Delay(iics_s->scltime);
    H_CC2640R2F_IICS_SDA_H(iics_s);
}

static void h_cc2640r2f_iics_writebyte(H_CC2640R2F_IICS_S *iics_s, \
                                        unsigned char data)
{
    unsigned char i = 0;
    
    //H_CC2640R2F_IICS_SCL_L();
    for(i = 0U; i < 8U; i++)
    {
        if(data & 0x80)
        {
            H_CC2640R2F_IICS_SDA_H(iics_s);
        }
        else
        {
            H_CC2640R2F_IICS_SDA_L(iics_s);
        }
        
        H_CC2640R2F_IICS_SCL_H(iics_s);
        data <<= 1;
        H_CC2640R2F_IICS_Delay(iics_s->scltime);
        H_CC2640R2F_IICS_SCL_L(iics_s);
    }
    
}

static unsigned char h_cc2640r2f_iics_readbyte(H_CC2640R2F_IICS_S *iics_s)
{
    unsigned char data = 0U;
    unsigned char i = 0U;
    
    h_cc2640r2f_iics_sda_in(iics_s);
    
    //H_CC2640R2F_IICS_SCL_L();
    //H_CC2640R2F_IICS_Delay(5);
    
    for(; i < 8; i++)
    {
        data <<= 1;
        
        H_CC2640R2F_IICS_SCL_H(iics_s);
        if(H_CC2640R2F_IICS_SDA_Read(iics_s))
        {
            data |= 0x01;
        }
        else
        {
            data |= 0x00;
        }
        // H_CC2640R2F_IICS_SCL_H(iics_s);
        
        H_CC2640R2F_IICS_SCL_L(iics_s);
        H_CC2640R2F_IICS_Delay(iics_s->scltime);
    }
    
    h_cc2640r2f_iics_sda_out(iics_s);
    return data;
}

static void h_cc2640r2f_iics_writebyte_slow(H_CC2640R2F_IICS_S *iics_s, \
                                                unsigned char data)
{
    unsigned char i = 0;

    //H_CC2640R2F_IICS_SCL_L();
    for(i = 0U; i < 8U; i++)
    {
        if(data & 0x80)
        {
            H_CC2640R2F_IICS_SDA_H(iics_s);
        }
        else
        {
            H_CC2640R2F_IICS_SDA_L(iics_s);
        }
        H_CC2640R2F_IICS_Delay(iics_s->scltime);
        H_CC2640R2F_IICS_Delay(iics_s->scltime);
        H_CC2640R2F_IICS_SCL_H(iics_s);
        data <<= 1;
        H_CC2640R2F_IICS_Delay(iics_s->scltime);
        H_CC2640R2F_IICS_Delay(iics_s->scltime);
        H_CC2640R2F_IICS_SCL_L(iics_s);
        H_CC2640R2F_IICS_Delay(iics_s->scltime);
        H_CC2640R2F_IICS_Delay(iics_s->scltime);
    }

}

static unsigned char h_cc2640r2f_iics_readbyte_slow(H_CC2640R2F_IICS_S *iics_s)
{
    unsigned char data = 0U;
    unsigned char i = 0U;

    h_cc2640r2f_iics_sda_in(iics_s);

    //H_CC2640R2F_IICS_SCL_L();
    //H_CC2640R2F_IICS_Delay(5);

    for(; i < 8; i++)
    {
        data <<= 1;

        H_CC2640R2F_IICS_SCL_L(iics_s);
        H_CC2640R2F_IICS_Delay(iics_s->scltime);
        // H_CC2640R2F_IICS_Delay(H_CC2640R2F_SCL_HTime);
        // H_CC2640R2F_IICS_Delay(H_CC2640R2F_SCL_HTime);

        H_CC2640R2F_IICS_SCL_H(iics_s);
        H_CC2640R2F_IICS_Delay(iics_s->scltime);
        // H_CC2640R2F_IICS_Delay(H_CC2640R2F_SCL_HTime);

        if(H_CC2640R2F_IICS_SDA_Read(iics_s))
        {
            data |= 0x01;
        }
        else
        {
            data |= 0x00;
        }

        H_CC2640R2F_IICS_Delay(iics_s->scltime);
//        H_CC2640R2F_IICS_SCL_H();
//
        H_CC2640R2F_IICS_SCL_L(iics_s);
        H_CC2640R2F_IICS_Delay(iics_s->scltime);
    }

    h_cc2640r2f_iics_sda_out(iics_s);
    return data;
}

static void h_cc2640r2f_iics_sendack(H_CC2640R2F_IICS_S *iics_s, \
                                        unsigned char ack)
{
    H_CC2640R2F_IICS_SCL_L(iics_s);
    if(H_CC2640R2F_IICS_ACK == ack)
    {
        H_CC2640R2F_IICS_SDA_L(iics_s);
    }
    else
    {
        H_CC2640R2F_IICS_SDA_H(iics_s);
    }
    // H_CC2640R2F_IICS_SCL_L(iics_s);
    
    H_CC2640R2F_IICS_SCL_H(iics_s);
    H_CC2640R2F_IICS_Delay(5);
    
    H_CC2640R2F_IICS_SCL_L(iics_s);
}

static void h_cc2640r2f_iics_sendack_slow(H_CC2640R2F_IICS_S *iics_s, \
                                            unsigned char ack)
{
    // H_CC2640R2F_IICS_SCL_L();
    // H_CC2640R2F_IICS_Delay(H_CC2640R2F_SCL_HTime);
    if(H_CC2640R2F_IICS_ACK == ack)
    {
        H_CC2640R2F_IICS_SDA_L(iics_s);
    }
    else
    {
        H_CC2640R2F_IICS_SDA_H(iics_s);
    }
    H_CC2640R2F_IICS_Delay(iics_s->scltime);
    // H_CC2640R2F_IICS_SCL_L();
    // H_CC2640R2F_IICS_Delay(H_CC2640R2F_SCL_HTime);
    
    H_CC2640R2F_IICS_SCL_H(iics_s);
    H_CC2640R2F_IICS_Delay(iics_s->scltime);
    
    H_CC2640R2F_IICS_SCL_L(iics_s);
    H_CC2640R2F_IICS_Delay(iics_s->scltime);
}

static signed char h_cc2640r2f_iics_readack(H_CC2640R2F_IICS_S *iics_s)
{
    unsigned char ack = 0U;
    h_cc2640r2f_iics_sda_in(iics_s);
    
    //H_CC2640R2F_IICS_SCL_L();
    //H_CC2640R2F_IICS_Delay(5);
    H_CC2640R2F_IICS_SCL_H(iics_s);
    
    if(H_CC2640R2F_IICS_SDA_Read(iics_s))
    {
        ack = H_CC2640R2F_IICS_NACK;
    }
    else
    {
        ack = H_CC2640R2F_IICS_ACK;
    }
    
    H_CC2640R2F_IICS_SCL_L(iics_s);
    h_cc2640r2f_iics_sda_out(iics_s);
    
    return ack;
}

static signed char h_cc2640r2f_iics_readack_slow(H_CC2640R2F_IICS_S *iics_s)
{
    unsigned char ack = 0U;
    h_cc2640r2f_iics_sda_in(iics_s);
    
    H_CC2640R2F_IICS_SCL_L(iics_s);
    H_CC2640R2F_IICS_Delay(iics_s->scltime);
    H_CC2640R2F_IICS_SCL_H(iics_s);
    H_CC2640R2F_IICS_Delay(iics_s->scltime);
    
    if(H_CC2640R2F_IICS_SDA_Read(iics_s))
    {
        ack = H_CC2640R2F_IICS_NACK;
    }
    else
    {
        ack = H_CC2640R2F_IICS_ACK;
    }
    H_CC2640R2F_IICS_Delay(iics_s->scltime);
    
    H_CC2640R2F_IICS_SCL_L(iics_s);
    H_CC2640R2F_IICS_Delay(iics_s->scltime);
    h_cc2640r2f_iics_sda_out(iics_s);
    
    return ack;
}

signed char H_CC2640R2F_IICS_ReadBuff(H_CC2640R2F_IICS_S *iics_s, \
                unsigned char addr, unsigned char reg, unsigned char *rdata, unsigned short len)
{
    signed char rslt = 0;
    
    h_cc2640r2f_iics_start(iics_s);
    h_cc2640r2f_iics_writebyte(iics_s, addr | H_CC2640R2F_IICS_WriteOP);
    
    rslt = h_cc2640r2f_iics_readack(iics_s);
    
    if(H_CC2640R2F_IICS_ACK != rslt)
    {
        h_cc2640r2f_iics_stop(iics_s);
        return (-1);
    }
    
    h_cc2640r2f_iics_writebyte(iics_s, reg);
    rslt = h_cc2640r2f_iics_readack(iics_s);
    
    h_cc2640r2f_iics_stop(iics_s);
    if(H_CC2640R2F_IICS_ACK != rslt)
    {
        return (-2);
    }
    
//    H_CC2640R2F_IICS_Delay(100);
    
    h_cc2640r2f_iics_start(iics_s);
    h_cc2640r2f_iics_writebyte(iics_s, addr | H_CC2640R2F_IICS_ReadOP);
    
    rslt = h_cc2640r2f_iics_readack(iics_s);
    
    if(H_CC2640R2F_IICS_ACK != rslt)
    {
        h_cc2640r2f_iics_stop(iics_s);
        return (-3);
    }
    
    len--;
    while(len--)
    {
        *rdata++ = h_cc2640r2f_iics_readbyte(iics_s);

//        H_CC2640R2F_IICS_Delay(100);
        h_cc2640r2f_iics_sendack(iics_s, H_CC2640R2F_IICS_ACK);

//        H_CC2640R2F_IICS_Delay(100);
    }
    
    *rdata++ = h_cc2640r2f_iics_readbyte(iics_s);
    h_cc2640r2f_iics_sendack(iics_s, H_CC2640R2F_IICS_NACK);
    
    h_cc2640r2f_iics_stop(iics_s);
    return 0;
}

signed char H_CC2640R2F_IICS_WriteByte(H_CC2640R2F_IICS_S *iics_s, \
            unsigned char addr, unsigned char reg, unsigned char data)
{
    signed char rslt = 0;
    
    h_cc2640r2f_iics_start(iics_s);
    h_cc2640r2f_iics_writebyte(iics_s, addr | H_CC2640R2F_IICS_WriteOP);
    
    rslt = h_cc2640r2f_iics_readack(iics_s);
    
    if(H_CC2640R2F_IICS_ACK != rslt)
    {
        h_cc2640r2f_iics_stop(iics_s);
        return (-1);
    }
    
    h_cc2640r2f_iics_writebyte(iics_s, reg);
    rslt = h_cc2640r2f_iics_readack(iics_s);
    
    if(H_CC2640R2F_IICS_ACK != rslt)
    {
        h_cc2640r2f_iics_stop(iics_s);
        return (-2);
    }
    
    
    h_cc2640r2f_iics_writebyte(iics_s, data);
    if(H_CC2640R2F_IICS_ACK != h_cc2640r2f_iics_readack(iics_s))
    {
        h_cc2640r2f_iics_stop(iics_s);
        return (-3);
    }
    
    h_cc2640r2f_iics_stop(iics_s);
    return 0;
}

signed char H_CC2640R2F_IICS_ReadBuff_Slow(H_CC2640R2F_IICS_S *iics_s, \
                unsigned char addr, unsigned char reg, unsigned char *rdata, unsigned short len)
{
    signed char rslt = 0;

    h_cc2640r2f_iics_start(iics_s);
    h_cc2640r2f_iics_writebyte_slow(iics_s, addr | H_CC2640R2F_IICS_WriteOP);

    rslt = h_cc2640r2f_iics_readack_slow(iics_s);

    if(H_CC2640R2F_IICS_ACK != rslt)
    {
        h_cc2640r2f_iics_stop(iics_s);
        return (-1);
    }

    h_cc2640r2f_iics_writebyte_slow(iics_s, reg);
    rslt = h_cc2640r2f_iics_readack_slow(iics_s);

    h_cc2640r2f_iics_stop(iics_s);
    if(H_CC2640R2F_IICS_ACK != rslt)
    {
        return (-2);
    }

//    H_CC2640R2F_IICS_Delay(100);

    h_cc2640r2f_iics_start(iics_s);
    h_cc2640r2f_iics_writebyte_slow(iics_s, addr | H_CC2640R2F_IICS_ReadOP);

    rslt = h_cc2640r2f_iics_readack_slow(iics_s);

    if(H_CC2640R2F_IICS_ACK != rslt)
    {
        h_cc2640r2f_iics_stop(iics_s);
        return (-3);
    }

    len--;
    while(len--)
    {
        *rdata++ = h_cc2640r2f_iics_readbyte_slow(iics_s);

        // H_CC2640R2F_IICS_Delay(100);
//       H_CC2640R2F_IICS_Delay(100);
        h_cc2640r2f_iics_sendack_slow(iics_s, H_CC2640R2F_IICS_ACK);

//        H_CC2640R2F_IICS_Delay(1000);
    }

    *rdata++ = h_cc2640r2f_iics_readbyte_slow(iics_s);
    h_cc2640r2f_iics_sendack_slow(iics_s, H_CC2640R2F_IICS_NACK);

    h_cc2640r2f_iics_stop(iics_s);
    return 0;
}

signed char H_CC2640R2F_IICS_WriteBuff(H_CC2640R2F_IICS_S *iics_s, \
                unsigned char addr, unsigned char reg, unsigned char *wdata, unsigned short len)
{
    signed char rslt = 0;
    
    h_cc2640r2f_iics_start(iics_s);
    h_cc2640r2f_iics_writebyte(iics_s, addr | H_CC2640R2F_IICS_WriteOP);
//    H_CC2640R2F_IICS_Delay(2500);
    
    rslt = h_cc2640r2f_iics_readack(iics_s);
//    H_CC2640R2F_IICS_Delay(2500);
    
    if(H_CC2640R2F_IICS_ACK != rslt)
    {
        h_cc2640r2f_iics_stop(iics_s);
        return (-1);
    }
    
    h_cc2640r2f_iics_writebyte(iics_s, reg);
//    H_CC2640R2F_IICS_Delay(2500);
    rslt = h_cc2640r2f_iics_readack(iics_s);
    
    if(H_CC2640R2F_IICS_ACK != rslt)
    {
        h_cc2640r2f_iics_stop(iics_s);
        return (-2);
    }
    
    
    while(len--)
    {
        h_cc2640r2f_iics_writebyte(iics_s, *wdata++);
        if(H_CC2640R2F_IICS_ACK != h_cc2640r2f_iics_readack(iics_s))
        {
            h_cc2640r2f_iics_stop(iics_s);
            return (-3);
        }
    }
    
    h_cc2640r2f_iics_stop(iics_s);
    return 0;
}

signed char H_CC2640R2F_IICS_WriteBuff_Slow(H_CC2640R2F_IICS_S *iics_s, \
                unsigned char addr, unsigned char reg, unsigned char *wdata, unsigned short len)
{
    signed char rslt = 0;

    h_cc2640r2f_iics_start(iics_s);
    h_cc2640r2f_iics_writebyte_slow(iics_s, addr | H_CC2640R2F_IICS_WriteOP);
//    H_CC2640R2F_IICS_Delay(2500);

    rslt = h_cc2640r2f_iics_readack(iics_s);
//    H_CC2640R2F_IICS_Delay(2500);

    if(H_CC2640R2F_IICS_ACK != rslt)
    {
        h_cc2640r2f_iics_stop(iics_s);
        return (-1);
    }

    h_cc2640r2f_iics_writebyte_slow(iics_s, reg);
//    H_CC2640R2F_IICS_Delay(2500);
    rslt = h_cc2640r2f_iics_readack(iics_s);

    if(H_CC2640R2F_IICS_ACK != rslt)
    {
        h_cc2640r2f_iics_stop(iics_s);
        return (-2);
    }


    while(len--)
    {
        h_cc2640r2f_iics_writebyte_slow(iics_s, *wdata++);
        if(H_CC2640R2F_IICS_ACK != h_cc2640r2f_iics_readack(iics_s))
        {
            h_cc2640r2f_iics_stop(iics_s);
            return (-3);
        }
    }

    h_cc2640r2f_iics_stop(iics_s);
    return 0;
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

signed char H_CC2640R2F_IICS0_WriteBuff_Slow(unsigned char addr, unsigned char reg, unsigned char *wdata, unsigned short len)
{
    return H_CC2640R2F_IICS_WriteBuff_Slow(&h_cc2640r2f_iics0_s, addr, reg, wdata, len);
}

signed char H_CC2640R2F_IICS0_ReadBuff_Slow(unsigned char addr, unsigned char reg, unsigned char *rdata, unsigned short len)
{
    return H_CC2640R2F_IICS_ReadBuff_Slow(&h_cc2640r2f_iics0_s, addr, reg, rdata, len);
}

signed char H_CC2640R2F_IICS1_WriteBuff(unsigned char addr, unsigned char reg, unsigned char *wdata, unsigned short len)
{
    return H_CC2640R2F_IICS_WriteBuff(&h_cc2640r2f_iics1_s, addr, reg, wdata, len);
}

signed char H_CC2640R2F_IICS1_ReadBuff(unsigned char addr, unsigned char reg, unsigned char *rdata, unsigned short len)
{
    return H_CC2640R2F_IICS_ReadBuff(&h_cc2640r2f_iics1_s, addr, reg, rdata, len);
}

signed char H_CC2640R2F_IICS1_WriteBuff_Slow(unsigned char addr, unsigned char reg, unsigned char *wdata, unsigned short len)
{
    return H_CC2640R2F_IICS_WriteBuff_Slow(&h_cc2640r2f_iics1_s, addr, reg, wdata, len);
}

signed char H_CC2640R2F_IICS1_ReadBuff_Slow(unsigned char addr, unsigned char reg, unsigned char *rdata, unsigned short len)
{
    return H_CC2640R2F_IICS_ReadBuff_Slow(&h_cc2640r2f_iics1_s, addr, reg, rdata, len);
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////


#include "h_cc2640r2f_simpletime.h"

signed char H_CC2640R2F_IICS_Test(void)
{
    int8_t rslt = 0;

    unsigned char addr = 0U;
    unsigned char data[5];

    // H_CC2640R2F_IICS1_Init();

    for(; addr < 255; addr++)
    {
        if(rslt = H_CC2640R2F_IICS1_ReadBuff(addr, 0x3A, data, 1) != 0)
        {
            //H_CC2640R2F_IICS_Debug("IICs read failed %d\r\n", rslt);
        }
        else
        {
            H_CC2640R2F_IICS_Debug("Data at 0x%02x is 0x%02x\r\n", addr, data[0]);
        }
        H_CC2640R2F_SimpleTime_Around_Delay(10);
    }

    
    return 0;
}

